#pragma once;

#include "Vector3.h";
#include "Vector4.h";

class Matrix4x4 : public D3DXMATRIX
{
	// Constructors

	public:

		Matrix4x4() : D3DXMATRIX()
		{
		}

		Matrix4x4(D3DMATRIX &matrix)
		{
			memcpy(&m, &matrix.m, sizeof(matrix.m));
		}

	// Methods

	public:

		inline void SetPosition(Vector3 const &pos)
		{
			m[3][0] = pos.x;
			m[3][1] = pos.y;
			m[3][2] = pos.z;
			m[3][3] = 1.0f;
		}

		inline void SetPosition(Vector4 const &pos)
		{
			m[3][0] = pos.x;
			m[3][1] = pos.y;
			m[3][2] = pos.z;
			m[3][3] = pos.w;
		}

		inline Vector3 GetPosition() const
		{
			return Vector3(m[3][0], m[3][1], m[3][2]);
		}

		inline Vector3 GetDirection() const
		{
			// Note - the following code can be used to double check the vector construction above.
			Matrix4x4 justRot = *this;
			
			justRot.SetPosition(Vector3(0.f,0.f,0.f));

			Vector3 forward = justRot.Transform(ForwardVector);

			return forward;
		}

		inline Vector3 GetYawPitchRoll() const
		{
			float yaw, pitch, roll;
	
			pitch = asin(-_32);

			double threshold = 0.001; // Hardcoded constant - burn him, he's a witch
			double test = cos(pitch);

			if(test > threshold) 
			{
				roll = atan2(_12, _22);
				yaw = atan2(_31, _33);
			}
			else 
			{
				roll = atan2(-_21, _11);
				yaw = 0.0;
			}

			return (Vector3(yaw, pitch, roll));
		}

		inline Vector3 Transform(Vector3 &vector3) const
		{
			Vector4 temp(vector3.x, vector3.y, vector3.z), out;

			D3DXVec4Transform(&out, &temp, this);

			return Vector3(out.x, out.y, out.z);
		}

		inline Vector4 Transform(Vector4 &vector4) const
		{
			Vector4 temp;

			D3DXVec4Transform(&temp, &vector4, this);

			return temp;
		}

		inline Matrix4x4 Inverse() const
		{
			Matrix4x4 out;

			D3DXMatrixInverse(&out, NULL, this);

			return out;
		}

	// Static

	public:

		static inline Matrix4x4& BuildYawPitchRollMatrix(
			const float yawRadians,
			const float pitchRadians,
			const float rollRadians)
		{
			Matrix4x4 matrix;

			D3DXMatrixRotationYawPitchRoll(
				&matrix,
				yawRadians,
				pitchRadians,
				rollRadians);

			return matrix;
		}

		static inline Matrix4x4& BuildLookAtMatrix(
			const Vector3 &eye,
			const Vector3 &at,
			const Vector3 &up)
		{
			Matrix4x4 matrix;

			D3DXMatrixLookAtRH(
				&matrix,
				&eye,
				&at,
				&up);

			return matrix;
		}

		static inline Matrix4x4& BuildTranslationMatrix(const Vector3 &position)
		{
			Matrix4x4 matrix = Matrix4x4::Identity;

			matrix.m[3][0] = position.x;
			matrix.m[3][1] = position.y;
			matrix.m[3][2] = position.z;

			return matrix;
		}

		static inline Matrix4x4& BuildRotationMatrixX(const float radians)
		{
			Matrix4x4 matrix;

			D3DXMatrixRotationX(&matrix, radians);

			return matrix;
		}

		static inline Matrix4x4& BuildRotationMatrixY(const float radians)
		{
			Matrix4x4 matrix;

			D3DXMatrixRotationY(&matrix, radians);

			return matrix;
		}

		static inline Matrix4x4& BuildRotationMatrixZ(const float radians)
		{
			Matrix4x4 matrix;

			D3DXMatrixRotationZ(&matrix, radians);

			return matrix;
		}

		static const Matrix4x4 Identity;
};

const Matrix4x4 Matrix4x4::Identity(D3DXMATRIX(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1));

inline Matrix4x4 operator * (const Matrix4x4 &a, const Matrix4x4 &b)
{
	Matrix4x4 out;

	D3DXMatrixMultiply(&out, &a, &b);

	return out;
}